#include "show.hpp"

inline float sqr(float x){return x*x;}
inline float row(Point a){return sqrt(sqr(a.x)+sqr(a.y));}
inline float phi(Point a){return atan2(a.y,a.x);}
inline float dist(Point x, Point y){return row(Point(x.x-y.x,x.y-y.y));}

using namespace std;

float Person::Threshold_TP = 8.0;

Trajectory::Trajectory()
{
}
void Trajectory::load(string src)
{
	std::ifstream inFile;
	inFile.open(src.c_str());
	char temp;

	if(inFile.is_open())
	{
		while(!inFile.eof())
		{
			inFile >> temp;
			if (temp != ':')
				continue;
			else
			{
				float x,y,width,height;
				inFile >> x >> temp >> y >> temp >> width >> temp >> height >> temp;
				x += width/2;
				y += height/2;
				Point l(x,y);
				location.push_back(l);
			}
		}
	}

	smooth(10);
	init();
}

void Trajectory::init()
{
	a.push_back(Point(0,0));
	for(size_t i=1;i<location.size();i++)
	{
		Point vv(location[i].x - location[i-1].x, location[i].y - location[i-1].y);
		v.push_back(vv);
		if(i>1)
		{
			Point aa(v[i-1].x - v[i-2].x, v[i-1].y - v[i-2].y);
			if(row(aa)>20)
				aa = Point(0,0);
			a.push_back(aa);
		}
	}
	v.push_back(Point(0,0));
	a.push_back(Point(0,0));
}

void Trajectory::smooth(int n)
{
	vector<Point> temp;
	for( unsigned i = 0; i < location.size(); i++ )
	{
		int count = 0;
		float sumx = 0;
		float sumy = 0;

		for( int j = (int)i - (int)n / 2; j <= (int)i + (int)n / 2; j++ )
		{
			if( j >= 0 && j < ( int )location.size())
			{
				sumx += location[i].x;
				sumy += location[i].y;
				count++;
			}
		}

		Point t(sumx / count, sumy / count);
		temp.push_back(t);
		//cout << t.x<< "," << t.y << " ";
	}
	//cout << endl;
	location.empty();
	for(size_t i=0;i<temp.size();i++)
		location.push_back(temp[i]);
}

Person::Person()
{
}

void Person::load(string src)
{
	std::ifstream inFile;
	inFile.open(src.c_str());
	char temp;

	if(inFile.is_open())
	{
		while(!inFile.eof())
		{
			inFile >> temp;
			if (temp != ':')
				continue;
			else
			{
				float x,y,width,height,sq,xx,yy;
				inFile >> x >> temp >> y >> temp >> width >> temp >> height;
				x += width/2;
				y += height/2;
				Point l(x,y);
				head.location.push_back(l);
				inFile >> temp >> xx >> temp >> yy;
				sq = sqrt(sqr(xx-x)+sqr(yy-y));
				Point p;
				if(sq > 0)
					p = Point((xx-x)/sq,(yy-y)/sq);
				else
					p = Point(0,0);
				headpose.push_back(p);
			}
		}
	}

	src.replace(5,1,"l");hand_l.load(src.c_str());
	src.replace(5,1,"r");hand_r.load(src.c_str());

	head.smooth(10);
	head.init();

	GetTP();
}

void Person::GetTP()
{
	//float c_f = 0,c_h = 0,c_a = 0;
	for(size_t i=1;i<headpose.size()-1;i++)
	{

		float a= row(head.a[i]);

		double an = phi(headpose[i-1]) + phi(headpose[i+1]) - 2*phi(headpose[i]);
		an = (an<0)?-an:an;
		if(an > 3.14159)
			an -= 3.14159;
		a += an*20;

		a += max(row(hand_l.a[i]),row(hand_r.a[i]))/1.5;

		Acc.push_back(a);
	}

	for(size_t i=0;i<Acc.size();i++)
	{
		float ts,te;
		if(Acc[i]>Threshold_TP)
		{
			if( i>0 && Acc[i-1]>Threshold_TP)
				te += 1;
			else
				ts=te=i;

			if(i == Acc.size()-1)
				TP.push_back(Point(ts,te));
		}
		else
		{
			if(i>0 && Acc[i-1]>Threshold_TP)
				TP.push_back(Point(ts,te));
		}
	}

	/*
	for(size_t i=0;i<TP.size();i++)
	{
		cout << TP[i].x << "," << TP[i].y << " ";
	}
	cout << endl;
	*/
}

double Person::ComputAccTpt(int ts, int te)
{
	if(ts==0)
		ts = 1;
	if(ts > te)
		return 0;
	float AccTpt = row(Point(head.v[ts].x - head.v[te].x, head.v[ts].y - head.v[te].y));
	//cout << ts << ":" << te << " " <<AccTpt << ",";
	float Acc_l = row(Point(hand_l.v[ts].x - hand_l.v[te].x, hand_l.v[ts].y - hand_l.v[te].y));
	float Acc_r = row(Point(hand_r.v[ts].x - hand_r.v[te].x, hand_r.v[ts].y - hand_r.v[te].y));
	AccTpt += max(Acc_l, Acc_r)/1.5;
	//cout << max(Acc_l, Acc_r)/4 << ",";
	float an;
	if(ts>0)
		an = phi(headpose[ts-1])  - phi(headpose[ts]);
	else
		an = phi(headpose[ts])  - phi(headpose[ts+1]);
	if(te<(int)headpose.size()-1)
		an += phi(headpose[te+1])  - phi(headpose[te]);
	else
		an += phi(headpose[te])  - phi(headpose[te-1]);
	an = (an<0)?-an:an;
	while(an > 3.14159)
		an -= 3.14159;
	AccTpt += an*20;
	//cout << an*30 << endl;
	return 	AccTpt;
}

Show::Show(Process *pro, States *_state):Observer(pro)
{
	state = _state;
	outFile.open("output/face.txt");
	outFile.setf(ios_base::floatfield,ios_base::fixed);
	num = 3;

	sigma_1 = 0.5;
	sigma_2 = 0.2;
	sigma_3 = 1.2;
	ssigma = 1/sigma_1/sqrt(2*3.1415);

	Alpha= 1.4;
	Beta= 1.1012;
	Gama= 1.05;

	p[0].load("00_b_h.txt");
	p[1].load("00_r_h.txt");
	p[2].load("00_w_h.txt");

	GetAtt();

	//cout << p[0].TP.size() << "," << p[1].TP.size() << "," << p[2].TP.size() << endl;
}

Show::~Show()
{
	Log("Release Show");
	outFile.close();
	_pro->Detach(this);
}

void Show::Update(const Image &image)
{
	image.CopyTo(out);

	int i = _pro->m_count;

	//if(i==1798)
	//	GetInfluence(i-150,i);
	if(i%150==0 && i>0 && i > t_count)
		GetInfluence(i-150,i);

	Point l[9];

	if (i > t_count && i%150==0)
	{
		outFile << "#" << i << ": " << endl;
	}


	for(int k=0;k<num;k++)
	{
		l[k] = p[k].head.location[i];
		l[num + 2*k] = p[k].hand_l.location[i];
		l[num + 2*k+1] = p[k].hand_r.location[i];
	}

	for(int j=0;j<num;j++)
	{
		out.Draw(I_Point(p[j].head.location[i].x,p[j].head.location[i].y),5);
		if(p[j].Att[i]!=9)
		{
			out.Draw(I_Point(l[p[j].Att[i]].x, l[p[j].Att[i]].y),3);
			out.Draw(I_Point(p[j].head.location[i].x,p[j].head.location[i].y),I_Point(l[p[j].Att[i]].x, l[p[j].Att[i]].y));
		}

		if(p[j].Acc[i]> Person::Threshold_TP)
			out.Draw(I_Rect(p[j].head.location[i].x - 30,p[j].head.location[i].y - 30,60,60));
		/*
		if(row(p[j].head.a[i])> Person::Threshold_TP)
			out.Draw(I_Rect(p[j].head.location[i].x - 10,p[j].head.location[i].y - 10,20,20));
		if(row(p[j].hand_l.a[i])/1.5> Person::Threshold_TP)
			out.Draw(I_Rect(p[j].hand_l.location[i].x - 10,p[j].hand_l.location[i].y - 10,20,20));
		if(row(p[j].hand_r.a[i])/1.5> Person::Threshold_TP)
			out.Draw(I_Rect(p[j].hand_r.location[i].x - 10,p[j].hand_r.location[i].y - 10,20,20));
		double an;
		if(i>0 ||i<1799)
			an = phi(p[j].headpose[i-1]) + phi(p[j].headpose[i+1]) - 2*phi(p[j].headpose[i]);
		else an = 0;
		an = (an<0)?-an:an;
		if(an > 3.14159)
			an -= 3.14159;
		if( an*20 > Person::Threshold_TP)
			out.Draw(I_Rect(p[j].head.location[i].x - 20,p[j].head.location[i].y - 20,40,40));
		*/
		/*
		if (_pro->m_count > t_count)
		{
			if ( p[j].Att[i] < num)
				outFile << j << " look at " << p[j].Att[i] << "'s head."<<endl;
			else
				outFile << j << " look at " << (p[j].Att[i]-num)/2 <<"'s " << ( p[j].Att[i]%2==0?"left":"right") << " hand."<< endl;
		}
		*/
	}

	if (_pro->m_count > t_count)
		t_count = _pro->m_count;
}


void Show::GetAtt()
{
	float sigma_1 = 0.5 , sigma_2 = 0.2 , sigma_3 = 1.2, ssigma = 1/sigma_1/sqrt(2*3.1415);

	for(int i=0;i<num;i++)
	{
		p[i].Att.push_back(0);p[i].Att.push_back(0);
	}

	for(size_t i=2;i<p[0].headpose.size()-1;i++)
	{
		float s[3][10];
		Point l[9];

		for(int k=0;k<num;k++)
		{
			l[k] = p[k].head.location[i];
			l[num + 2*k] = p[k].hand_l.location[i];
			l[num + 2*k+1] = p[k].hand_r.location[i];
		}

		for(int j=0;j<num;j++)
		{
			for(int k=0;k<num*3;k++)
			{
				float xx = l[k].x - p[j].head.location[i].x, yy = l[k].y - p[j].head.location[i].y, sq=sqrt(sqr(xx)+sqr(yy));
				float _sq=(sq>0?sq:1);
				Point tt(xx/_sq, yy/_sq);
				s[j][k] = ssigma*exp(-((sqr(p[j].headpose[i].x - tt.x) + sqr(p[j].headpose[i].y - tt.y))/2/sigma_1));
				s[j][k] *= 1/(1+exp(-sigma_2*sq));

				if(j!=k)
					s[j][k] *= sigma_3;
			}
			s[j][9] = ssigma;
		}

		for(int j=0;j<num;j++)
		{
			float max=-1;int flag = 0;
			for(int k=0;k<num*3+1;k++)
			{
				if (s[j][k]>max)
				{
					max = s[j][k];
					flag = k;
				}
			}
			p[j].Att.push_back(flag);
		}
	}
}

int Show::GetDeltaT( int personto, int personfrom, int timet )
{
	int deltat = ( int )99999;

	for(size_t i = 0; i < p[personfrom].head.location.size(); i++ )
	{
		if( abs((int)i- timet) < deltat && p[personfrom].Acc[i]> Person::Threshold_TP)
		{
			deltat = abs((int)i - timet );
		}
	}
	return deltat;
}

void Show::GetInfluence(int ts, int te)
{
	if (ts<0)
		ts=0;
	if (te>(int)p[0].head.location.size())
		te = p[0].head.location.size();
	AllInfluence.resize( num );
	for( unsigned i = 0; i < AllInfluence.size(); i++)
	{
		AllInfluence[i].resize( num );
		for( unsigned j = 0; j < AllInfluence[i].size(); j++)
		{
			AllInfluence[i][j].TrajInfluence.resize(p[i].TP.size());
		}
	}

	for(size_t i = 0; i < AllInfluence.size(); i++ )
	{
		for(size_t j = 0; j < AllInfluence[i].size(); j++ )
		{
			if(i==j)
				continue;
			double spatialaccumulated = 0;
			double temporalaccumulated = 0;
			double maxaccumulated = 0;
			int count = 0;

			for( unsigned k = 0; k < AllInfluence[i][j].TrajInfluence.size(); k++ )
			{
				if(p[i].TP[k].x + p[i].TP[k].y< 2.*ts)
					continue;
				if(p[i].TP[k].x, p[i].TP[k].y >= 2.*te)
					continue;

				double AccTpt = p[i].ComputAccTpt(p[i].TP[k].x, p[i].TP[k].y);

				double DTpt = 999999;

				for( int f = p[i].TP[k].x; f <= p[i].TP[k].y; f++ )
				{
					if( row(Point(p[i].head.location[f].x - p[j].head.location[f].x, p[i].head.location[f].y - p[j].head.location[f].y)) < DTpt )
					{
						DTpt = row(Point(p[i].head.location[f].x - p[j].head.location[f].x, p[i].head.location[f].y - p[j].head.location[f].y));
					}
				}

				DTpt /= 40;
				//AccTpt = 1;

				double ATpt = pow( Alpha, -DTpt );

				double BTpt = pow( Beta, -DTpt * pow( Gama, GetDeltaT( i, j, (p[i].TP[k].x + p[i].TP[k].y) / 2 ) ) );

				//outFile << i << "__"<< p[i].TP[k].x << "__"<< p[i].TP[k].y <<":" << AccTpt << "," << ATpt << "," << BTpt <<endl;

				AllInfluence[i][j].TrajInfluence[k].SetInfluence( AccTpt * ATpt, AccTpt * BTpt, j, i );

				//compute the average influence from person j to person i
				spatialaccumulated += AccTpt * ATpt;
				temporalaccumulated += AccTpt * BTpt;
				maxaccumulated += max( AccTpt * ATpt, AccTpt * BTpt );
				count++;
			}

			if( count > 0 )
			{
				AllInfluence[i][j].Average.SetInfluence( spatialaccumulated / count, temporalaccumulated / count, j, i );
				outFile << i << " from " << j << ":" << spatialaccumulated / count << "," << temporalaccumulated / count << "," <<  maxaccumulated / count << endl;
			}
		}
	}

	outFile << endl;

	for(size_t i = 0; i < (size_t)num; i++ )
	{
		int l[10];
		for(int k=0;k<10;k++)
			l[k]=0;
		for(int t = ts;t<te;t++)
		{
			l[p[i].Att[t]]++;
		}
		cout << " " ;
		for(int k=0;k<10;k++)
			cout << l[k] << ",";
		cout << endl;
		outFile << "Single: "<<(float)(l[i] + l[num+i*2] + l[num+i*2+1] + l[9])/(te-ts)*100 << "%" ;
		for(int k=0;k<num;k++)
		{
			if(k==i)
				continue;
			outFile << " Others: " << (float)(l[k] + l[num + k*2] + l[num + k*2+1])/(te-ts)*100 << "% ,";
		}
		outFile << endl;
	}
	outFile << endl;
}
